#ifndef _SERVO_KINCO_H__
#define _SERVO_KINCO_H__

#include <motor/motor.h>

#include <std_msgs/msg/int8.hpp>
#include <std_msgs/msg/int16.hpp>
#include <std_msgs/msg/int32.hpp>
#include <std_msgs/msg/u_int8.hpp>
#include <std_msgs/msg/u_int16.hpp>
#include <std_msgs/msg/u_int32.hpp>
#include <std_msgs/msg/float64.hpp>
#include <agv_srvs/srv/set_canopen_entry.hpp>


namespace ros_canopen_driver {

struct PDO_CACHE_KINCO
{ 
    std::atomic<int8_t>      mode{0};
    std::atomic<uint16_t>    error_code{0};
    std::atomic<int32_t>     current_pos{0};
    std::atomic<int32_t>     current_vel{0};
    std::atomic<uint16_t>    status_word{0};
    std::atomic<uint32_t>    digital_input{0};
    std::atomic<uint32_t>    profile_vel{0};
    std::atomic<int32_t>     target_pos{0};
    std::atomic<int16_t>     current{0};
    std::atomic<double>      timestamp{0.0};
};

class ServoKinco : public Motor
{
public:
    ServoKinco(std::shared_ptr<rclcpp::Node> node, const std::string name);
    ~ServoKinco();

    int Motor_Init();

    bool Motor_Enable(bool onFlag);

    bool Motor_InitParam();

    bool Motor_HomeMachine();

    bool Motor_InitVmode();

    bool Motor_InitPmode();

    bool Motor_SwitchMode(int8_t mode);

 
    void Motor_SetAcc(uint32_t acc);

    void Motor_SetDec(uint32_t dec);

    void Motor_SetPmodeVel(uint32_t vel);

    void Motor_SetTargetVelRad(double vel);

    void Motor_SetTargetVelEnc(int vel);

    void Motor_SetTargetPosRad(double pos);

    void Motor_SetTargetPosEnc(int pos);

    void Motor_ForkLift(bool up, int vel);


    MotorState Motor_GetState();

    double Motor_GetCurrentVel();

    double Motor_GetCurrentPos();

    double Motor_GetCurrent();

    uint16_t Motor_GetStatusWord();

    uint16_t Motor_GetErrCode();

    int8_t Motor_GetMode();


    void Motor_Brake(bool on);

    void Motor_Shutdown();

    bool Motor_Halt();

    bool Motor_Recover();

    double Motor_GetTimestamp() { return pc_.timestamp;}

    std::string Motor_Err2userErr(const uint16_t& motorErr);

private:
    // Canopen *pCanopen;
    SERVOMOTOR_PORT m_servo_motor;
    int preTargetEnc;
    std::atomic<MotorState> motorState;

    struct PDO_CACHE_KINCO pc_;

    // rpdo pub
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt16>>   set_control_word_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int32>>    set_vel_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int32>>    set_pos_pub;
    // sdo pub
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt16>>   set_control_word_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt32>>   set_di_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt8>>    set_home_offset_mode_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt32>>   set_homing_switch_speed_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt32>>   set_homing_zero_speed_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int32>>    set_home_offset_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt32>>   set_homing_acc_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int8>>     set_homing_method_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int8>>     set_mode_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt32>>   set_acc_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt32>>   set_dec_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt32>>   set_p_vel_sdo_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int32>>    set_pos_sdo_pub;

    rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr     mode_sub;
    rclcpp::Subscription<std_msgs::msg::UInt16>::SharedPtr   errcode_sub;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr    current_pos_sub;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr    current_vel_sub;
    rclcpp::Subscription<std_msgs::msg::UInt16>::SharedPtr   status_word_sub;
    rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr   di_sub;
    rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr   p_vel_sub;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr    target_pos_sub;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr  timestamp_sub;
    rclcpp::Subscription<std_msgs::msg::Int16>::SharedPtr    current_sub;

    rclcpp::Client<agv_srvs::srv::SetCanopenEntry>::SharedPtr  set_entry_cln;

    
    void mode_sub_callback(const std_msgs::msg::Int8::ConstSharedPtr& msg);
    void errcode_sub_callback(const std_msgs::msg::UInt16::ConstSharedPtr& msg);
    void current_pos_sub_callback(const std_msgs::msg::Int32::ConstSharedPtr& msg);
    void current_vel_sub_callback(const std_msgs::msg::Int32::ConstSharedPtr& msg);
    void status_word_sub_callback(const std_msgs::msg::UInt16::ConstSharedPtr& msg);
    void di_sub_callback(const std_msgs::msg::UInt32::ConstSharedPtr& msg);
    void p_vel_sub_callback(const std_msgs::msg::UInt32::ConstSharedPtr& msg);
    void target_pos_sub_callback(const std_msgs::msg::Int32::ConstSharedPtr& msg);
    void timestamp_sub_callback(const std_msgs::msg::Float64::ConstSharedPtr& msg);
    void current_sub_callback(const std_msgs::msg::Int16::ConstSharedPtr& msg);

    /* <---  add end   */
    
    void servo_modeSet(int8_t mode);

    void servo_homeOffsetModeSet(uint8_t mode);

    void servo_homeOffsetSet(int homeOffset);

    void servo_homeMethodSet(int homeMethod);

    void servo_homeSpeedHSet(uint32_t homeSpeedH);

    void servo_homeSpeedLSet(uint32_t homeSpeedL);

    void servo_homeAccSet(uint32_t homeAcc);

    void servo_homeStart();

    int servo_encoderGet();

    void servo_PRelMove(int data);

    int servo_velGet();

    uint32_t servo_PvelGet();

    int servo_VvelGet();

    uint32_t servo_DiStatusGet();

    void servo_readDiStatus();

    void servo_ErrClear();

    bool Motor_InitIVmode();
};

}
#endif
